
clc;clear;
getSession;
sess.connect_device;
ziMaster = sess.getDevice('dev3543');
ziSlave = sess.getDevice('dev3560');
%%
condition.BmainVolt = 6.05;
condition.wpACAmp = 0.2;
condition.wpACfreq = 113;
condition.drvFreq = 101e3;
condition.wpOsc = '1';
condition.drvOsc = '0';
condition.BmainCH = '2';%3543 aux3
condition.XctrlCH = '0';%3543 aux1
condition.YctrlCH = '1';%3543 aux2
condition.XdemodCH = '2';%demodulator 3
condition.YdemodCH = '3';%demodulator 4

%%

ziMaster.loadSettings('D:\Code\Automeasurement\AutoMeasurementApp\+Table1\ZiSettings\wp_phaseX3543');
ziSlave.loadSettings('D:\Code\Automeasurement\AutoMeasurementApp\+Table1\ZiSettings\wp_phaseX3560');
setMainField(ziMaster,condition.BmainCH,condition.BmainVolt);
ziMaster.setOscFreq(condition.drvOsc,condition.drvFreq);
ziMaster.setOscFreq(condition.wpOsc,condition.wpACfreq);

ziMaster.setAuxOutScal(condition.XctrlCH,condition.wpACAmp);

phaseY_c=SwpCoursePhaseY(ziSlave);
rangeYfine=[phaseY_c-4,phaseY_c+4];
phaseY_f = SwpFinePhaseY(ziSlave,rangeYfine);
condition.phaseY = phaseY_f;
pause(0.1);
ziMaster.loadSettings('D:\Code\Automeasurement\AutoMeasurementApp\+Table1\ZiSettings\wp_phaseY3543');
ziSlave.loadSettings('D:\Code\Automeasurement\AutoMeasurementApp\+Table1\ZiSettings\wp_phaseY3560');

setMainField(ziMaster,condition.BmainCH,condition.BmainVolt);
ziMaster.setOscFreq(condition.drvOsc,condition.drvFreq);
ziMaster.setOscFreq(condition.wpOsc,condition.wpACfreq);
phaseX_c=SwpCoursePhaseX(ziSlave);
rangeXfine=[phaseX_c-4,phaseX_c+4];
phaseX_f = SwpFinePhaseX(ziSlave,rangeXfine);
condition.phaseX = phaseX_f;

pause(0.1);
%%

ziMaster.setDemodPhase(condition.XdemodCH,condition.phaseX);
ziMaster.setDemodPhase(condition.YdemodCH,condition.phaseY);
ziSlave.disableOutAmplitude('0');
%%

[condition.slopeX,condition.zeroX] = SwpSlope(ziMaster, condition.XctrlCH,condition.XdemodCH);
ziMaster.setAuxOutOffset(condition.XctrlCH,condition.zeroX);
[condition.slopeY,condition.zeroY] = SwpSlope(ziMaster, condition.YctrlCH,condition.YdemodCH);
ziMaster.setAuxOutOffset(condition.YctrlCH,condition.zeroY);






function setMainField(ziMaster,BmainCH,BmainVolt)
    v0 = ziMaster.getAuxOutValue(BmainCH);
    dv = min( abs(BmainVolt-v0), 0.01);
    dv = sign(BmainVolt-v0)*dv;
%    for v = v0:dv:volt
    for v = v0:dv:BmainVolt
        ziMaster.setAuxOutOffset(BmainCH, v);
        pause(0.01);
    end
    ziMaster.setAuxOutOffset(BmainCH, BmainVolt);
end

function PhaseCourse = SwpCoursePhaseY (zi)
swpr=zi.sweepModule();
%%

swpr.setControl( 'start', 0, 'stop', 180, 'samplecount', 50);
swpr.setFilter('bandwidthcontrol', 1, 'bandwidth', 5, 'order', 4);
swpr.setSettling('settling_time', 0);
swpr.setStatistics('averaging_sample', 1);
ziDAQ('set',swpr.hdl,'sweep/gridnode','/dev3543/demods/3/phaseshift');
swpr.reset();
swpr.subscribeChannel('3', {'x','y', 'r'});

swpr_data = swpr.run();
swpr.unsubscribeChannel();

swpdata=swpr_data.getNodeData('3',{'x','y', 'r'});

PhaseCourse = getphase(swpdata); 
end

function PhaseCourse = SwpCoursePhaseX (zi)
swpr=zi.sweepModule();
%%

swpr.setControl( 'start', 0, 'stop', 180, 'samplecount', 50);
swpr.setFilter('bandwidthcontrol', 1, 'bandwidth', 5, 'order', 4);
swpr.setSettling('settling_time', 0);
swpr.setStatistics('averaging_sample', 1);
ziDAQ('set',swpr.hdl,'sweep/gridnode','/dev3543/demods/3/phaseshift');
swpr.reset();
swpr.subscribeChannel('2', {'x','y', 'r'});

swpr_data = swpr.run();
swpr.unsubscribeChannel();

swpdata=swpr_data.getNodeData('2',{'x','y', 'r'});

PhaseCourse = getphase(swpdata); 
end


function Phasefine = SwpFinePhaseY (zi,range)
swpr=zi.sweepModule();
%%

swpr.setControl( 'start', range(1), 'stop', range(2), 'samplecount', 100);
swpr.setFilter('bandwidthcontrol', 1, 'bandwidth', 5, 'order', 4);
swpr.setSettling('settling_time', 0);
swpr.setStatistics('averaging_sample', 1);
ziDAQ('set',swpr.hdl,'sweep/gridnode','/dev3543/demods/3/phaseshift');
swpr.reset();
swpr.subscribeChannel('3', {'x','y', 'r'});

swpr_data = swpr.run();
swpr.unsubscribeChannel();

swpdata=swpr_data.getNodeData('3',{'x','y', 'r'});

Phasefine = getphase(swpdata); 
end
function Phasefine = SwpFinePhaseX (zi,range)
swpr=zi.sweepModule();
%%

swpr.setControl( 'start', range(1), 'stop', range(2), 'samplecount', 100);
swpr.setFilter('bandwidthcontrol', 1, 'bandwidth', 5, 'order', 4);
swpr.setSettling('settling_time', 0);
swpr.setStatistics('averaging_sample', 1);
ziDAQ('set',swpr.hdl,'sweep/gridnode','/dev3543/demods/3/phaseshift');
swpr.reset();
swpr.subscribeChannel('2', {'x','y', 'r'});

swpr_data = swpr.run();
swpr.unsubscribeChannel();

swpdata=swpr_data.getNodeData('2',{'x','y', 'r'});

Phasefine = getphase(swpdata); 
end

function phasecross =getphase(data)
stddata = std(data.value,0,2);
[~,idx] = min(stddata);
phasecross = data.grid(idx);
end

function [slope,zerop] = SwpSlope(zi,CHswp,CHdemod)
gridch=sprintf('auxouts/%s/offset',CHswp);
swpr=zi.sweepModule();
%%
swpr.setControl('gridnode',  gridch, 'start', -0.3, 'stop', 0.3, 'samplecount', 50);
swpr.setFilter('bandwidthcontrol', 1, 'bandwidth', 200.0, 'order', 8);
swpr.setSettling('settling_time', 0.01);
swpr.setStatistics('averaging_sample', 100);

swpr.reset();
swpr.subscribeChannel(CHdemod, 'x');
swpr_data = swpr.run();
swpr.unsubscribeChannel();

fitdata = swpr_data.getNodeData(CHdemod, 'x');
[slope,zerop]= fitzero(fitdata.grid,fitdata.value);

end
function [slope,zerop] = fitzero(datax,datay)
p=polyfit(datax,datay,1);
slope = p(1);
zerop = -p(2)./p(1);
end


